/*
 * GPS_PID.cpp
 *
 *  Created on: Feb 9, 2014
 *      Author: ppks
 */

#include "common.h"
#include "GPS_common.h"
#include "FlightController_Common.h"

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
PID_PARAM poshold_PID_PARAM;
PID_PARAM posholdRate_PID_PARAM;
PID_PARAM nav_PID_PARAM;

PID posholdPID[2];
PID posholdRatePID[2];
PID navPID[2];

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: GPS_SetPIDs
********************************************************************************
* Summary:
*  Sets local PID values for GPS module from global FC Config.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_SetPIDs(void)
{
#if defined(GPS_SERIAL_ENABLED)  || defined(GPS_FROM_OSD_ENABLED)
    poshold_PID_PARAM.kP   = (float)FC_Config.PID[PIDPOS].P8 / 100.0;
    poshold_PID_PARAM.kI   = (float)FC_Config.PID[PIDPOS].I8 / 100.0;
    poshold_PID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;

    posholdRate_PID_PARAM.kP   = (float)FC_Config.PID[PIDPOSR].P8 / 10.0;
    posholdRate_PID_PARAM.kI   = (float)FC_Config.PID[PIDPOSR].I8 / 100.0;
    posholdRate_PID_PARAM.kD   = (float)FC_Config.PID[PIDPOSR].D8 / 1000.0;
    posholdRate_PID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;

    nav_PID_PARAM.kP   = (float)FC_Config.PID[PIDNAVR].P8 / 10.0;
    nav_PID_PARAM.kI   = (float)FC_Config.PID[PIDNAVR].I8 / 100.0;
    nav_PID_PARAM.kD   = (float)FC_Config.PID[PIDNAVR].D8 / 1000.0;
    nav_PID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif /* defined(GPS_SERIAL_ENABLED)  || defined(GPS_FROM_OSD_ENABLED) */
}

/*******************************************************************************
* Function Name: GPS_ResetPID
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void GPS_ResetPID(struct GPS_PID_* pid)
{
    pid->integrator = 0;
    pid->last_input = 0;
    pid->lastderivative = 0;
}

/*******************************************************************************
* Function Name: GPS_GetPID_P
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
int32_t GPS_GetPID_P(int32_t error, struct PID_PARAM_* pid)
{
    return (float)error * pid->kP;
}

/*******************************************************************************
* Function Name: GPS_GetPID_I
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
int32_t GPS_GetPID_I(int32_t error, float* dt, struct GPS_PID_* pid, struct PID_PARAM_* pid_param)
{
    pid->integrator += ((float)error * pid_param->kI) * (*dt);
    pid->integrator = constrain(pid->integrator,-pid_param->Imax,pid_param->Imax);
    return pid->integrator;
}

/*******************************************************************************
* Function Name: GPS_GetPID_I
********************************************************************************
* Summary:
*	Examples for _filter calculated as 1 / ( 2 * PI * f_cut ) :
*   f_cut = 10 Hz -> _filter = 15.9155e-3
*   f_cut = 15 Hz -> _filter = 10.6103e-3
*   f_cut = 20 Hz -> _filter =  7.9577e-3
*   f_cut = 25 Hz -> _filter =  6.3662e-3
*   f_cut = 30 Hz -> _filter =  5.3052e-3
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
int32_t GPS_GetPID_D(int32_t input, float* dt, struct GPS_PID_* pid, struct PID_PARAM_* pid_param)
{
    /* dt in milliseconds */
    pid->derivative = (input - pid->last_input) / (*dt);

    /* Low pass filter cut off frequency for derivative calculation. */
    float filter = 7.9577e-3; /* filter output for f_cut = 20Hz */

    /* discrete low pass filter, cuts out the
     * high frequency noise that can drive the controller crazy */
    pid->derivative = pid->lastderivative + (*dt / ( filter + *dt)) * (pid->derivative - pid->lastderivative);

    /* update state */
    pid->last_input = input;
    pid->lastderivative = pid->derivative;

    /* add in derivative component */
    return pid_param->kD * pid->derivative;
}
